#include <unordered_map>
#include <mutex>
#include <thread>
#include <condition_variable>
#include <vector>
#include <chrono>
#include <iostream>
#include <atomic>
#include <memory>

// 缓存接收到的消息，key 为 IMEI，value 为最新的消息
std::unordered_map<std::string, std::shared_ptr<v2v::UdpPathPointListForV2V>> message_map;
std::mutex map_mutex;  // 保护队列的互斥锁
std::condition_variable cv;  // 用于通知消费线程
std::atomic<bool> stop_threads(false);  // 停止线程的标志

// 模拟转发函数，批量转发数据
void ForwardV2VMessages(std::vector<std::shared_ptr<v2v::UdpPathPointListForV2V>>& messages) {
    for (const auto& udpPathPointList : messages) {
        try {
            auto path_point_list = udpPathPointList->v2vmessage();
            if (!path_point_list) {
                std::cerr << "UdpPathPointListForV2V does not contain PathPointListForV2V." << std::endl;
                continue;
            }

            // 创建 FlatBuffers builder
            flatbuffers::FlatBufferBuilder builder;

            // 处理 IMEI 字符串
            auto imei_offset = builder.CreateString(path_point_list->imei()->c_str());

            // 处理路径点
            std::vector<flatbuffers::Offset<v2v::pathPointForV2V>> points;
            for (auto point : *path_point_list->pointList()) {
                points.push_back(v2v::CreatepathPointForV2V(
                    builder,
                    point->utm_x(),
                    point->utm_y(),
                    point->heading()
                ));
            }
            auto points_offset = builder.CreateVector(points);

            // 处理 is_passright_imei 和 not_passright_imei
            std::vector<flatbuffers::Offset<flatbuffers::String>> passright_imei_vector;
            std::vector<flatbuffers::Offset<flatbuffers::String>> not_passright_imei_vector;

            if (path_point_list->is_passright_imei()) {
                for (auto imei : *path_point_list->is_passright_imei()) {
                    passright_imei_vector.push_back(builder.CreateString(imei->str()));
                }
            }
            if (path_point_list->not_passright_imei()) {
                for (auto imei : *path_point_list->not_passright_imei()) {
                    not_passright_imei_vector.push_back(builder.CreateString(imei->str()));
                }
            }
            auto is_passright_imei_offset = builder.CreateVector(passright_imei_vector);
            auto not_passright_imei_offset = builder.CreateVector(not_passright_imei_vector);

            // 创建 PathPointListForV2V 消息
            auto serializedMessage = v2v::CreatepathPointlistForV2V(
                builder, 
                path_point_list->timestamp(),
                imei_offset,
                path_point_list->utm_x(),
                path_point_list->utm_y(),
                path_point_list->height(),
                path_point_list->heading(),
                path_point_list->dev_length(),
                path_point_list->dev_width(),
                path_point_list->dev_height(),
                path_point_list->speed(),
                path_point_list->lateral_angular_velocity(),
                path_point_list->full_load_status(),
                path_point_list->path_count(),
                path_point_list->path_interval(),
                points_offset,
                is_passright_imei_offset,
                not_passright_imei_offset
            );

            builder.Finish(serializedMessage);

            // 发送消息
            auto pub_msg = std::make_shared<SimMessage>(
                kSimV2VOutputNodeName, kSimV2VNodeName, builder);
            pub(pub_msg);
            std::cout << "Forwarded PathPointListForV2V message via pub()." << std::endl;

        } catch (const std::exception &e) {
            std::cerr << "Error processing V2V message: " << e.what() << std::endl;
        }
    }
}

// 消费线程，定时消费数据
void ConsumerThread() {
    while (!stop_threads) {
        std::this_thread::sleep_for(std::chrono::milliseconds(50));  // 每 50ms 消费一次

        std::vector<std::shared_ptr<v2v::UdpPathPointListForV2V>> batch;
        {
            std::unique_lock<std::mutex> lock(map_mutex);
            // 将所有消息放入批量中
            for (auto& entry : message_map) {
                batch.push_back(entry.second);
            }
            message_map.clear();  // 清空队列，准备接收新的数据
        }

        if (!batch.empty()) {
            // 批量转发接收到的消息
            ForwardV2VMessages(batch);

            // 从 map 中移除已经消费的消息
            for (const auto& entry : batch) {
                const std::string imei = entry->v2vmessage()->imei()->str();
                std::unique_lock<std::mutex> lock(map_mutex);
                message_map.erase(imei);  // 删除已消费的消息
            }
        }
    }
}

// 接收线程，模拟接收数据并放入队列
void udpReceiveflatbuffer() {
    double v2v_range = 50.0; // 范围，单位为米
    double packet_loss_rate = 0.1; // 丢包率，10%

    v2v_range = std::stod(FLAGS_v2v_range);
    packet_loss_rate = std::stod(FLAGS_v2vDropProbability);

    char buffer[2048];
    struct sockaddr_in sender_addr;
    socklen_t addr_len = sizeof(sender_addr);

    // 非阻塞接收一次数据
    ssize_t bytes_received = recvfrom(udp_socket_, buffer, sizeof(buffer), MSG_DONTWAIT,
                                       (struct sockaddr *)&sender_addr, &addr_len);

    if (bytes_received > 0) {
        try {
            // 使用 FlatBuffers 解析收到的数据
            auto udpPathPointList = v2v::GetUdpPathPointListForV2V(buffer);
            if (!udpPathPointList) {
                std::cerr << "Failed to parse UdpPathPointListForV2V." << std::endl;
                return;
            }

            auto path_point_list = udpPathPointList->v2vmessage();
            if (!path_point_list) {
                std::cerr << "UdpPathPointListForV2V does not contain PathPointListForV2V." << std::endl;
                return;
            }

            // 模拟丢包
            double randValue = static_cast<double>(rand()) / RAND_MAX;
            if (randValue < packet_loss_rate) {
                std::cout << "Simulated packet loss. Dropping received message." << std::endl;
                return;
            }

            // 提取关键信息
            double obs_lon = path_point_list->utm_x();
            double obs_lat = path_point_list->utm_y();
            const std::string &cloud_ip = udpPathPointList->cloudip()->str();

            // 检查是否在范围内，且与当前车辆的 cloudip 相同
            if (current_vehicle_state_.cloudip == cloud_ip) {
                std::unique_lock<std::mutex> lock(map_mutex);
                // 使用 IMEI 号作为 key，如果该车的数据已经存在，则更新为最新的数据
                std::string imei = path_point_list->imei()->str();
                message_map[imei] = udpPathPointList;  // 替换旧的数据
                lock.unlock();

                cv.notify_one();  // 通知消费者有新的数据
            }

        } catch (const std::exception &e) {
            std::cerr << "Error parsing received V2V message: " << e.what() << std::endl;
        }
    } else if (bytes_received < 0 && errno != EWOULDBLOCK && errno != EAGAIN) {
        perror("UDP receive failed");
    }
}

// 程序入口
int main() {
    // 启动消费线程
    std::thread consumer_thread(ConsumerThread);

    // 启动接收线程，模拟接收数据
    while (!stop_threads) {
        udpReceiveflatbuffer();  // 每次调用一次接收
        std::this_thread::sleep_for(std::chrono::milliseconds(10));  // 模拟接收间隔
    }

    // 等待消费线程结束
    consumer_thread.join();

    return 0;
}
